#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdbool.h>
#include "i2c.h"
#include "motor_control.h"


#define CHECK_BIT(var,pos) ((var) & (1<<(pos)))

#define TWI_SLA       0x10  


#define CHANNELS_CONFIG 0x00
#define CHANNEL_A_SPEED 0x01
#define CHANNEL_B_SPEED 0x02
	

#define BRAKE 2
#define FREE  1
#define RUN	  0

#define CONFIG_MASK 0x7
#define DIR_MASK 0x08

#define CHANNELA_DECAL 0
#define CHANNELB_DECAL 4


int state ;
int overflow_count = 0 ;

uint8_t regs [] = {0, 0, 0};



/**
 * Three bytes have to be received for a full command. This enum is used to
 * indicate what part of the command we are waiting for.
 */
typedef enum {
    WAIT_FOR_ADDRESS,     /**< first byte is the address */
    WAIT_FOR_VALUE
} ReadCommandState;

/**
 * Holds one command that is received via i2c. The command consists of an
 * address (number of output channel) and a 16-bit value. The state is used to
 * indicate what part of the next command we are waiting for.
 */
typedef struct {
    uint8_t address;         /**< number of output channel (between 0 and
                               CHANNEL_COUNT-1 */
    uint8_t value;          /**< value to be assigned to the channel (between
                               0 and 128*128-1 = 16383 */
    ReadCommandState state;  /**< what are we waiting for? */

} Command;

/** the next command is built in this variable */
Command command = {0, 0, WAIT_FOR_ADDRESS};

bool commandReceived = false ;


void setupLedA(void){
	DDRD |= (1 << PD1);
	PORTD |= (1 << PD1);
}

void toggleLedA(void){
	PORTD = PORTD ^ (1 << PD1);
}

void setLedA(void){
	PORTD |= (1 << PD1);
}


void clearLedA(void){
	PORTD &= ~(1 << PD1);
}



void parseConfig(uint8_t config){
	uint8_t tempa ;
	uint8_t tempb ;
	setChannelADir(CHECK_BIT(config,3));
	setChannelBDir(CHECK_BIT(config,7));
	
	tempa = (config >> CHANNELA_DECAL) & CONFIG_MASK ;
	tempb = (config >> CHANNELB_DECAL) & CONFIG_MASK ;
	switch(tempa){
		case 0 :
			setChannelARun();
			break ;
		case 1 :
			setChannelAFree();
			break;
		case 2 :
			setChannelABrake();
			break;
		default :
			break ;
	}
	switch(tempb){
		case 0 :
			setChannelBRun();
			break;
		case 1 :
			setChannelBFree();
			break;
		case 2 :
			setChannelBBrake();
			break;
		default :
			break ;
	}

}



void evaluate_i2c_input(u08 receiveDataLength, u08* recieveData) {
	int i ;
	for(i = 0 ; i < receiveDataLength ; i ++ ){
        // we have input
        u08 byte_received = recieveData[i];
        switch(command.state){
            case WAIT_FOR_ADDRESS:
                if (byte_received & 0x80) {
                    // bit 7 is set -> address received
                    command.address = (byte_received & 0x7F);
                    command.state = WAIT_FOR_VALUE;
                }
                // do nothing if this byte didn't look like an address
                break;
			case WAIT_FOR_VALUE:
                command.value = byte_received ;
				command.state = WAIT_FOR_ADDRESS;
				commandReceived = true ;
                break;
        }
	}
}


u08 read_callback(u08 transmitDataLengthMax, u08* transmitData){
    switch(command.state){
		case WAIT_FOR_VALUE:
			if(command.address < 4){
            	transmitData[0] = regs[command.address] ;
			}
			else{
				transmitData[0] =  0x00 ;
			}
			command.state = WAIT_FOR_ADDRESS;
            break;
		default :
			break ;
    }
	return 1 ;
}


ISR(TIMER0_OVF_vect)
{
	if(overflow_count > 20){
		setSpeedB(0x00);
		setSpeedA(0x00);
		clearLedA();
	}else{
		overflow_count ++ ;
	}
}

void init_timeout(void){
	TIMSK0 |= (1 << TOIE0)  ;
	TCCR0B |= (1 < CS00) | (1 << CS02) ;
	TCNT0 = 0x00 ;
	overflow_count = 0 ;

}

void reset_timeout(){
	TCNT0 = 0x00 ;
	overflow_count = 0 ;
	setLedA();
}


int main(void){
	setupPWM();
	setChannelADir(1);
	setChannelBDir(1);
	setupLedA();
	i2cInit();
	i2cSetLocalDeviceAddr(TWI_SLA, 1);
	i2cSetSlaveReceiveHandler(evaluate_i2c_input);
	i2cSetSlaveTransmitHandler(read_callback);
	DDRB &= ~(1 << PB4);
	DDRD &= ~(1 << PD7) ;
	init_timeout();
	while(1){
		if(commandReceived){
			reset_timeout() ;
			if(command.state == WAIT_FOR_ADDRESS){
				switch(command.address){
					case CHANNELS_CONFIG :
						parseConfig(command.value);
						regs[0] = command.value ;
						break ;
					case CHANNEL_A_SPEED :
						setSpeedA(command.value);
						regs[1] = command.value ;
						break ;
					case CHANNEL_B_SPEED :
						setSpeedB(command.value);
						regs[2] = command.value ;
						break ;
					default :
						break ;
		
				}
			}
			commandReceived = false ;
		}
		_delay_ms(1);
	}
	return 0 ;
}


